#include "toroidal.h"
#include <stdio.h>
#include "math.h"


fiq world::inertial(fiq phies, field vA2)
{
    int n = 4;
    double* iphi = phies.get_fi();
    double* qphi = phies.get_fq();

    double* uri = new double[Nx*Ny];
    double* uzi = new double[Nx*Ny];
    double* urq = new double[Nx*Ny];
    double* uzq = new double[Nx*Ny];

    partialx(field(iphi), uri);
    partialy(field(iphi), uzi);
    partialx(field(qphi), urq);
    partialy(field(qphi), uzq);


    
    double* valfven = vA2.getdata();
    for (int i = 0; i < Nx*Ny; i++)
    {
        if (valfven[i] == 0.0)
            continue;
        double ivalf = 1.0/valfven[i];
        uri[i] = uri[i]*ivalf;
        uzi[i] = uzi[i]*ivalf;
        urq[i] = urq[i]*ivalf;
        uzq[i] = uzq[i]*ivalf;
    }
    
    double* buffer = new double[Nx*Ny];
    double* newfi = new double[Nx*Ny];
    
    partialx(field(uri), buffer);
    for (int i = 0; i < Nx*Ny; i++)
        newfi[i] = newfi[i] + buffer[i];
    
    partialy(field(uzi), buffer);
    for (int i = 0; i < Nx*Ny; i++)
        newfi[i] = newfi[i] + buffer[i];

    double* newfq = new double[Nx*Ny];
    partialx(field(urq), buffer);
    for (int i = 0; i < Nx*Ny; i++)
        newfq[i] = newfq[i] + buffer[i];
    
    partialy(field(uzq), buffer);
    for (int i = 0; i < Nx*Ny; i++)
        newfq[i] = newfq[i] + buffer[i];

    
    return fiq(newfi, newfq, n);
}

fiq world::field_line_bending(fiq phies, vfield b, field Bsca)
{
    int n = 4;
    double* br = b.getfr();
    double* bp =b.getfphi();
    double* bz =b.getfz();

    vfiq * Esf = new vfiq(4);
    phies.gradient(Esf);
    double* iphi = phies.get_fi();
    double* qphi = phies.get_fq();

    double* paragradi = new double[Nx*Ny];
    double* paragradq = new double[Nx*Ny];
    double ivdr = 1.0/dr;
    double ivdz = 1.0/dz;
    // parallel gradient
    for(int i = 0; i < Nx*Ny; i++)
    {
        int ix = i/Ny;
        int iy = i%Ny;
        int up = (iy == 0 ? i : i-1);
        int dn = (iy == Ny-1 ? i : i+1);
        int lf = (ix == 0 ? i : i-Ny);
        int rg = (ix == Nx-1 ? i : i+Ny);

        paragradi[i] += br[i]*(iphi[lf]-iphi[rg])/(double)((rg-lf)/Ny)*ivdr;
        paragradi[i] += bz[i]*(iphi[up]-iphi[dn])/(double)((dn-up))*ivdz;
        paragradi[i] += bp[i]*qphi[i]*n;

        paragradq[i] += br[i]*(qphi[lf]-qphi[rg])/(double)((lf-rg)/Ny)*ivdr;
        paragradq[i] += bz[i]*(qphi[up]-qphi[dn])/(double)((up-dn))*ivdz;
        paragradq[i] += -bp[i]*iphi[i]*n;
    }
    // nabla square laplacian operator
    double* nbsqi = new double[Nx*Ny];
    double* nbsqq = new double[Nx*Ny];

    for(int i = 0; i < Nx*Ny; i++)
    {
        int ix = i/Ny;
        int iy = i%Ny;
        if(ix == 0) {
            nbsqi[i] += (paragradi[i+2*Ny]-2*paragradi[i+Ny]+paragradi[i])/(2*dr*dr);
            nbsqq[i] += (paragradq[i+2*Ny]-2*paragradq[i+Ny]+paragradq[i])/(2*dr*dr);
        }
        else if (ix == Nx-1) {
            nbsqi[i] += (-paragradi[i-2*Ny]+2*paragradi[i-Ny]-paragradi[i])/(2*dr*dr);
            nbsqq[i] += (-paragradq[i-2*Ny]+2*paragradq[i-Ny]-paragradq[i])/(2*dr*dr);
        }
        else{
            nbsqi[i] += (paragradi[i+Ny]+paragradi[i-Ny]-2*paragradi[i])/(dr*dr);
            nbsqq[i] += (paragradq[i+Ny]+paragradq[i-Ny]-2*paragradq[i])/(dr*dr);
        }
        if (iy ==0) {
            nbsqq[i] += (paragradq[i+2]-2*paragradq[i+1]+paragradq[i])/(2*dz*dz);
            nbsqi[i] += (paragradi[i+2]-2*paragradi[i+1]+paragradi[i])/(2*dz*dz);
        }
        else if (iy == Ny-1) {
            nbsqq[i] += (-paragradq[i+1]+2*paragradq[i-1]-paragradq[i])/(2*dz*dz);
            nbsqi[i] += (-paragradi[i+1]+2*paragradi[i-1]-paragradi[i])/(2*dz*dz);
        }
        else{
            nbsqq[i] += (paragradq[i+1]+paragradq[i-1]-2*paragradq[i])/(dz*dz);
            nbsqi[i] += (paragradi[i+1]+paragradi[i-1]-2*paragradi[i])/(dz*dz);
        }
    }

    double * Bs = Bsca.getdata();
    for(int i = 0; i < Nx*Ny; i++)
    {
        if (Bs[i] == 0.0)
            continue;
        nbsqi[i] = nbsqi[i]/Bs[i];
        nbsqq[i] = nbsqq[i]/Bs[i];
    }
 
    // parallel gradient
    for(int i = 0; i < Nx*Ny; i++)
    {
        int ix = i/Ny;
        int iy = i%Ny;
        int up = (iy == 0 ? i : i-1);
        int dn = (iy == Ny-1 ? i : i+1);
        int lf = (ix == 0 ? i : i-Ny);
        int rg = (ix == Nx-1 ? i : i+Ny);
        paragradi[i] += br[i]*(nbsqi[lf]-nbsqi[rg])/(double)((lf-rg)/Ny)*ivdr;
        paragradi[i] += bz[i]*(nbsqi[up]-nbsqi[dn])/(double)((up-dn))*ivdz;
        paragradi[i] += bp[i]*nbsqq[i]*n;

        paragradq[i] += br[i]*(nbsqq[lf]-nbsqq[rg])/(double)((lf-rg)/Ny)*ivdr;
        paragradq[i] += bz[i]*(nbsqq[up]-nbsqq[dn])/(double)((up-dn))*ivdz;
        paragradq[i] += -bp[i]*nbsqi[i]*n;
    }
    return fiq(paragradi,paragradq,n);
}
